use r2r::*;
use rand::rngs::StdRng;
use rand::Rng;
use rand::SeedableRng;
use std::sync::{Arc, Mutex};
use tokio;
use tokio::sync::Mutex as AsyncMutex;
use tokio::time::Instant;
use tokio::time::{self, sleep, Duration};

use r2r::chapt4_interfaces::srv::Patrol;
use r2r::rcl_interfaces::srv::SetParameters;

async fn patrol_client(arc_node: Arc<Mutex<r2r::Node>>) {
    // Limiting the scope when locking the arc
    let (client, mut timer, nl) = {
        let mut node = arc_node.lock().unwrap();

        // 如果 create_client/create_wall_timer 返回的类型是 owned（没有借用 node），
        // 这里得到的 client/timer 就是可以跨 await 使用的 owned 值。
        let client = node
            .create_client::<Patrol::Service>("/patrol", qos::QosProfile::default())
            .unwrap();
        let timer = node
            .create_wall_timer(std::time::Duration::from_secs(10))
            .unwrap();

        // 把 logger 转成 String（或 clone 一个可独立持有的 Logger），
        // 避免保留对 node 的借用。
        let nl = node.logger().to_string();

        (client, timer, nl)
    }; // node 的 MutexGuard 在这里被释放

    let service_available = r2r::Node::is_available(&client).unwrap();

    r2r::log_info!(&nl, "waiting for service...");
    service_available.await.unwrap();
    r2r::log_info!(&nl, "service available.");

    loop {
        let mut rng = StdRng::from_entropy();
        let mut request = Patrol::Request::default();

        // 11 X 11
        request.target_x = rng.gen_range(0.0..11.0) as f32;
        request.target_y = rng.gen_range(0.0..11.0) as f32;

        r2r::log_info!(&nl, "请求巡逻：({}, {})", request.target_x, request.target_y);

        // let mut client = client.lock().await;
        match client.request(&request).unwrap().await {
            Ok(response) => {
                if response.result == 1 {
                    r2r::log_info!(&nl, "目标点处理成功");
                } else if response.result == 0 {
                    r2r::log_info!(&nl, "目标点处理失败");
                }
            }
            Err(e) => {
                r2r::log_error!(&nl, "请求失败: {:?}", e);
            }
        }
        timer.tick().await.unwrap();
    }
}

#[tokio::main]
async fn main() -> std::result::Result<(), Box<dyn std::error::Error>> {
    let ctx = r2r::Context::create()?;
    let node = r2r::Node::create(ctx, "patrol_client", "")?;

    let arc_node = Arc::new(Mutex::new(node));
    let nl = arc_node.lock().unwrap().logger().to_string();

    let node_clone = arc_node.clone();
    let param_client = node_clone
        .lock()
        .unwrap()
        .create_client::<SetParameters::Service>(
            "/turtle_controller/set_parameters",
            qos::QosProfile::default(),
        )?;

    let waiting = r2r::Node::is_available(&param_client)?;

    r2r::log_info!(
        nl.as_str(),
        "waiting for /turtle_controller/set_parameters service..."
    );

    match tokio::time::timeout(Duration::from_secs(2), waiting).await {
        Ok(Ok(())) => {
            r2r::log_info!(
                nl.as_str(),
                "service /turtle_controller/set_parameters available."
            );

            let _k = 1.5;
            let mut param = r2r::rcl_interfaces::msg::Parameter::default();
            param.name = "k".to_string();
            param.value.type_ = r2r::rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE as u8;
            param.value.double_value = _k;

            let mut request = r2r::rcl_interfaces::srv::SetParameters::Request::default();
            request.parameters.push(param);

            match param_client.request(&request)?.await {
                Ok(response) => {
                    for result in response.results {
                        if result.successful {
                            r2r::log_info!(nl.as_str(), "参数k 已修改为：{}", _k);
                        } else {
                            r2r::log_warn!(nl.as_str(), "参数k 失败原因：{}", result.reason);
                        }
                    }
                }
                Err(e) => r2r::log_error!(nl.as_str(), "参数修改失败: {:?}", e),
            }
        }
        Ok(Err(e)) => {
            // waiting future 本身返回 Err
            r2r::log_info!(
                nl.as_str(),
                "service /turtle_controller/set_parameters error: {}",
                e.to_string()
            );
            // return Err(Box::new(e));
        }
        Err(_) => {
            // 超时
            r2r::log_info!(
                nl.as_str(),
                "service /turtle_controller/set_parameters timed out."
            );
        }
    }

    let node_clone = arc_node.clone();

    tokio::task::spawn(async move { patrol_client(node_clone).await });

    let handle = tokio::task::spawn_blocking(move || loop {
        {
            arc_node
                .lock()
                .unwrap()
                .spin_once(std::time::Duration::from_millis(10));
        }
        std::thread::sleep(std::time::Duration::from_millis(100))
    });

    handle.await?;

    Ok(())
}
